#include "Quaternion.h"

using namespace maths;

Quaternion::Quaternion()
{
	__asm
	{
		xorps xmm0, xmm0
		mov eax, dword ptr this
		movaps xmmword ptr [eax], xmm0
	}
}

Quaternion::Quaternion(float a_X, float a_Y, float a_Z, float a_W)
{
	m_Data[0] = a_X;
	m_Data[1] = a_Y;
	m_Data[2] = a_Z;
	m_Data[3] = a_W;
}

Quaternion::Quaternion(const Quaternion& a_Q)
{
	__asm
	{
		mov eax, dword ptr a_Q
		movaps xmm0, xmmword ptr [eax]
		mov eax, dword ptr this
		movaps xmmword ptr [eax], xmm0
	}
}

float& Quaternion :: operator [](int a_Index) 
{
	return m_Data[a_Index];
}

Quaternion& Quaternion :: operator  = (const Quaternion& a_Q)
{
	__asm
	{
		mov eax, dword ptr a_Q
		movaps xmm0, xmmword ptr [eax]
		mov eax, dword ptr this
		movaps xmmword ptr [eax], xmm0
	}
	return *this;
}

Quaternion& Quaternion :: operator += (const Quaternion& a_Q)
{
	__asm
	{
		mov eax, dword ptr a_Q
		movaps xmm0, xmmword ptr [eax]
		mov eax, dword ptr this
		movaps xmm1, xmmword ptr [eax]
		addps xmm0, xmm1
		movaps xmmword ptr [eax], xmm0
	}
	return *this;
}

Quaternion& Quaternion :: operator -= (const Quaternion& a_Q)
{
	__asm
	{
		mov eax, dword ptr a_Q
		movaps xmm0, xmmword ptr [eax]
		mov eax, dword ptr this
		movaps xmm1, xmmword ptr [eax]
		subps xmm0, xmm1
		movaps xmmword ptr [eax], xmm0
	}
	return *this;
}

Quaternion& Quaternion :: operator *= (const Quaternion& a_Q)
{
	__asm
	{
		mov eax, dword ptr a_Q
		movaps xmm7, xmmword ptr [eax]   //xmm7 = w z y x
		pshufd xmm4, xmm7, 0x9C          //xmm4 = z y w x
		pshufd xmm5, xmm7, 0xB1          //xmm5 = z w x y
		pshufd xmm6, xmm7, 0x1E          //xmm6 = x y w z
		pshufd xmm7, xmm7, 0x63          //xmm7 = y z x w

		mov eax, dword ptr this
		movaps xmm3, xmmword ptr [eax]   //xmm3 = d c b a
		pshufd xmm0, xmm3, 0x00          //xmm0 = a a a a
		pshufd xmm1, xmm3, 0x55          //xmm1 = b b b b
		pshufd xmm2, xmm3, 0xAA          //xmm2 = c c c c
		pshufd xmm3, xmm3, 0xFF          //xmm3 = d d d d

		mulps  xmm0, xmm4                //xmm0 = az ay aw ax
		mulps  xmm1, xmm5                //xmm1 = bz bw bx by
		mulps  xmm2, xmm6                //xmm2 = cx cy cw cz
		mulps  xmm3, xmm7                //xmm3 = dy dz dx dw

		addsubps xmm0, xmm3              //xmm3 = az+dy ay-dz aw+dx ax-dw
		pshufd xmm0, xmm0, 0xD8          //xmm3 = az+dy aw+dx ay-dz ax-dw

		addsubps xmm0, xmm2              //xmm3 = az+cx+dy aw-cy+dx ay+cw-dz ax-cz-dw
		pshufd xmm0, xmm0, 0xB4          //xmm3 = aw-cy+dx az+cx+dy ay+cw-dz ax-cz-dw

		addsubps xmm0, xmm1              //xmm3 = aw+bz-cy+dx az-bw+cx+dy ay+bx+cw-dz ax-by-cz-dw

		movaps xmmword ptr [eax], xmm0
	}
	return *this;
}

Quaternion& Quaternion :: operator *= (const float a_K)
{
	__asm
	{
		mov eax, dword ptr this
		movaps xmm0, xmmword ptr [eax]
		movss   xmm1, a_K
		pshufd xmm1, xmm1, 00h
		mulps  xmm0, xmm1
		movaps xmmword ptr [eax], xmm0
	}
	return *this;
}

Quaternion& Quaternion :: operator /= (const float a_K)
{
	__asm
	{
		mov eax, dword ptr this
		movaps xmm0, xmmword ptr [eax]
		movss   xmm1, a_K
		pshufd xmm1, xmm1, 00h
		divps  xmm0, xmm1
		movaps xmmword ptr [eax], xmm0
	}
	return *this;
}

bool Quaternion :: operator != (const Quaternion& a_Q)
{
	return ((fabs(m_Data[0] - a_Q.m_Data[0]) > _FLOAT_EPS_) ||
		(fabs(m_Data[1] - a_Q.m_Data[1]) > _FLOAT_EPS_) ||
		(fabs(m_Data[2] - a_Q.m_Data[2]) > _FLOAT_EPS_) ||
		(fabs(m_Data[3] - a_Q.m_Data[3]) > _FLOAT_EPS_));
}


bool Quaternion :: operator == (const Quaternion& a_Q)
{
	return ((fabs(m_Data[0] - a_Q.m_Data[0]) < _FLOAT_EPS_) &&
		(fabs(m_Data[1] - a_Q.m_Data[1]) < _FLOAT_EPS_) &&
		(fabs(m_Data[2] - a_Q.m_Data[2]) < _FLOAT_EPS_) &&
		(fabs(m_Data[3] - a_Q.m_Data[3]) < _FLOAT_EPS_));
}

Quaternion Quaternion :: operator + (const Quaternion& a_Q) 
{
	Quaternion l_Result;
	__asm
	{
		mov eax, dword ptr this
		movaps xmm0, xmmword ptr [eax]
		mov eax, dword ptr a_Q
		movaps xmm1, xmmword ptr [eax]
		addps  xmm0, xmm1
		movaps xmmword ptr [l_Result], xmm0
	}
	return l_Result;
}

Quaternion Quaternion :: operator - (const Quaternion& a_Q)
{
	Quaternion l_Result;
	__asm
	{
		mov eax, dword ptr this
		movaps xmm0, xmmword ptr [eax]
		mov eax, dword ptr a_Q
		movaps xmm1, xmmword ptr [eax]
		subps  xmm0, xmm1
		movaps xmmword ptr [l_Result], xmm0
	}
	return l_Result;
}

Quaternion Quaternion :: operator * (const Quaternion& a_Q)
{
	Quaternion l_Result;
	__asm
	{
		mov eax, dword ptr a_Q
		movaps xmm7, xmmword ptr [eax]   //xmm7 = w z y x
		pshufd xmm4, xmm7, 0x9C          //xmm4 = z y w x
		pshufd xmm5, xmm7, 0xB1          //xmm5 = z w x y
		pshufd xmm6, xmm7, 0x1E          //xmm6 = x y w z
		pshufd xmm7, xmm7, 0x63          //xmm7 = y z x w

		mov eax, dword ptr this
		movaps xmm3, xmmword ptr [eax]   //xmm3 = d c b a
		pshufd xmm0, xmm3, 0x00          //xmm0 = a a a a
		pshufd xmm1, xmm3, 0x55          //xmm1 = b b b b
		pshufd xmm2, xmm3, 0xAA          //xmm2 = c c c c
		pshufd xmm3, xmm3, 0xFF          //xmm3 = d d d d

		mulps  xmm0, xmm4                //xmm0 = az ay aw ax
		mulps  xmm1, xmm5                //xmm1 = bz bw bx by
		mulps  xmm2, xmm6                //xmm2 = cx cy cw cz
		mulps  xmm3, xmm7                //xmm3 = dy dz dx dw

		addsubps xmm0, xmm3              //xmm3 = az+dy ay-dz aw+dx ax-dw
		pshufd xmm0, xmm0, 0xD8          //xmm3 = az+dy aw+dx ay-dz ax-dw

		addsubps xmm0, xmm2              //xmm3 = az+cx+dy aw-cy+dx ay+cw-dz ax-cz-dw
		pshufd xmm0, xmm0, 0xB4          //xmm3 = aw-cy+dx az+cx+dy ay+cw-dz ax-cz-dw

		addsubps xmm0, xmm1              //xmm3 = aw+bz-cy+dx az-bw+cx+dy ay+bx+cw-dz ax-by-cz-dw

		movaps xmmword ptr [l_Result], xmm0
	}
	return l_Result;
}

Quaternion Quaternion :: operator * (const float a_K)
{
	Quaternion l_Result;
	__asm
	{
		mov eax, dword ptr this
		movaps xmm0, xmmword ptr [eax]
		movss   xmm1, a_K
		pshufd xmm1, xmm1, 00h
		mulps  xmm0, xmm1
		movaps xmmword ptr [l_Result], xmm0
	}
	return l_Result;
}

Quaternion Quaternion :: operator / (const float a_K)
{
	Quaternion l_Result;
	__asm
	{
		mov eax, dword ptr this
		movaps xmm0, xmmword ptr [eax]
		movss   xmm1, a_K
		pshufd xmm1, xmm1, 00h
		divps  xmm0, xmm1
		movaps xmmword ptr [l_Result], xmm0
	}
	return l_Result;
}

//functions

Quaternion& Quaternion :: Invert()
{
	__asm
	{
		mov eax, dword ptr this
		movaps xmm0, xmmword ptr [eax]
		xorps xmm1, xmm1
		subps xmm1, xmm0
		movaps xmmword ptr [eax], xmm1
	}
	return *this;
}

Quaternion& Quaternion :: Normalize()
{
	__asm
	{
		mov eax, dword ptr this
		movaps  xmm0, xmmword ptr [eax]
		movaps  xmm1, xmm0
		mulps   xmm0, xmm0
		haddps  xmm0, xmm0
		haddps  xmm0, xmm0
		rsqrtps xmm0, xmm0
		mulps   xmm1, xmm0
		movaps  xmmword ptr [eax], xmm1
	}
	return *this;
}

Quaternion& Quaternion :: SetLength (float a_Length)
{
	__asm
	{
		mov eax, dword ptr this
		movaps  xmm0, xmmword ptr [eax]
		movaps  xmm1, xmm0
		mulps   xmm0, xmm0
		haddps  xmm0, xmm0
		haddps  xmm0, xmm0
		rsqrtps xmm0, xmm0
		mulps   xmm1, xmm0

		movss   xmm0, a_Length
		pshufd  xmm0, xmm0, 0h
		mulps   xmm1, xmm0
		movaps  xmmword ptr [eax], xmm1
	}
	return *this;
}

/*void Quaternion :: RotateAround(const Quaternion& a_Target, float a_Angle)
{

Quaternion v = *this;
float x_, y_, z_, temp1;
float rad_angle = float(a_Angle * 3.1415926 / 360.0);
float c = cos(rad_angle);
float s = sin(rad_angle);

temp1 = sqrt(1 - a_Target.m_Data[2] * a_Target.m_Data[2]);
if (temp1 != 0)
{
x_ = v.m_Data[0] * a_Target.m_Data[1] / temp1 - v.m_Data[1] * a_Target.m_Data[0] / temp1;
y_ = v.m_Data[0] * a_Target.m_Data[0] / temp1 + v.m_Data[1] * a_Target.m_Data[1] / temp1;
v.m_Data[0] = x_;
v.m_Data[1] = y_;
y_ = v.m_Data[1] * a_Target.m_Data[2] - v.m_Data[2] * temp1;
z_ = v.m_Data[1] * temp1 + v.m_Data[2] * a_Target.m_Data[2];
v.m_Data[1] = y_;
v.m_Data[2] = z_;
x_ = c * v.m_Data[0] - s * v.m_Data[1];
y_ = c * v.m_Data[1] + s * v.m_Data[0];
v.m_Data[0] = x_;
v.m_Data[1] = y_;
y_ = v.m_Data[1] * a_Target.m_Data[2] + v.m_Data[2] * temp1;
z_ = -v.m_Data[1] * temp1 + v.m_Data[2] * a_Target.m_Data[2];
v.m_Data[1] = y_;
v.m_Data[2] = z_;
x_ = v.m_Data[0] * a_Target.m_Data[1] / temp1 + v.m_Data[1] * a_Target.m_Data[0] / temp1;
y_ = -v.m_Data[0] * a_Target.m_Data[0] / temp1 + v.m_Data[1] * a_Target.m_Data[1] / temp1;
v.m_Data[0] = x_;
v.m_Data[1] = y_;
v.Normalize();
}
else
{
x_ = c * v.m_Data[0] - s * v.m_Data[1];
y_ = c * v.m_Data[1] + s * v.m_Data[0];
v.m_Data[0] = x_;
v.m_Data[1] = y_;
};
*this = v;
}*/
/*
void Quaternion::QuatProduct(const Quaternion& a_V1, const Quaternion& a_V2, Quaternion& a_Result)
{
const int l_SignMask[4][4] =
{
{         0,          0, 0x80000000, 0},
{0x80000000,          0,          0, 0},
{         0, 0x80000000,          0, 0},
{0x80000000, 0x80000000, 0x80000000, 0}
};
__asm
{
movaps xmm3, xmmword ptr [a_V1]  //xmm3 =  x  y  z  t

pshufd xmm0, xmm3, 0x1B          //xmm0 =  t  z  y  x
movaps xmm4, xmmword ptr [l_SignMask]
xorps   xmm0, xmm4                //xmm0 =  t  z -y  x

pshufd xmm1, xmm3, 0x4E          //xmm1 =  z  t  x  y
movaps xmm4, xmmword ptr [l_SignMask + 16]
xorps   xmm1, xmm4                //xmm1 = -z  t  x  y

pshufd xmm2, xmm3, 0xB1          //xmm2 =  y  x  t  z
movaps xmm4, xmmword ptr [l_SignMask + 32]
xorps   xmm2, xmm4                //xmm2 =  y -x  t  z

movaps xmm4, xmmword ptr [l_SignMask + 48]
xorps   xmm3, xmm4                //xmm3 = -y -x -t  z

movaps xmm4, xmmword ptr [a_V2]  //xmm4 =  a  b  c  d

mulps  xmm0, xmm4
mulps  xmm1, xmm4
mulps  xmm2, xmm4
mulps  xmm3, xmm4

haddps xmm0, xmm1
haddps xmm2, xmm3
haddps xmm0, xmm2

movaps xmmword ptr [a_Result], xmm0
}

__asm
{
movaps xmm3, xmmword ptr [a_V1]  //xmm3 =  a b c d
pshufd xmm0, xmm3, 0xFF          //xmm0 = a a a a
pshufd xmm1, xmm3, 0xAA          //xmm1 = b b b b
pshufd xmm2, xmm3, 0x55          //xmm2 = c c c c
pshufd xmm3, xmm3, 0x00          //xmm3 = d d d d

movaps xmm7, xmmword ptr [a_V2]  //xmm7 =  x y z t
pshufd xmm4, xmm7, 0x1B          //xmm4 = t z y x
pshufd xmm5, xmm7, 0x72          //xmm5 = z x t y
pshufd xmm6, xmm7, 0x2D          //xmm6 = t y x z
pshufd xmm7, xmm7, 0x78          //xmm7 = z x y t

mulps  xmm0, xmm4                //xmm0 = at az ay ax
mulps  xmm1, xmm5                //xmm1 = bz bx bt by
mulps  xmm2, xmm6                //xmm2 = ct cy cx cz
mulps  xmm3, xmm7                //xmm3 = dz dx dy dt

addsubps xmm3, xmm2              //xmm3 = dz+ct dx-cy dy+cx dt-cz
pshufd xmm3, xmm3, 0xB4          //xmm3 = dx-cy dz+ct dy+cx dt-cz

addsubps xmm3, xmm1              //xmm3 = dx-cy+bz dz+ct-bx dy+cx+bt dt-cz-by
pshufd xmm3, xmm3, 0xD8          //xmm3 = dx-cy+bz dy+cx+bt dz+ct-bx dt-cz-by

addsubps xmm3, xmm0              //xmm3 = dx-cy+bz+at dy+cx+bt-az dz+ct-bx+ay dt-cz-by-ax

movaps xmmword ptr [a_Result], xmm3
}
}*/